
#include <cstdlib>

#include <QLineF>
#include <QLinkedList>
#include <QPointF>
#include <QRectF>
#include <QtOpenGL>
#include <QVector>

#include "ai/astar.h"
#include "ai/map.h"

Map::Map()
    : m_bIsValid(false)
{

}

Map::~Map()
{
}

bool Map::isValid() const
{
    return m_bIsValid;
}

const Object& Map::object() const
{
    return m_kMapObject;
}

const Quadtree< Edge3D >& Map::walls() const
{
    return m_kQuadtreeWalls;
}

const Quadtree< Face >& Map::walkables() const
{
    return m_kQuadtreeWalkables;
}

bool Map::load(const QString &rkFileName)
{
    // Clearing.
    m_kGates.clear();
    m_kWalls.clear();
    m_kGraph.clear();
    m_kQuadtreeWalls.clear();
    m_kQuadtreeWalkables.clear();

    // Load object file and check for errors.
    if(!m_kMapObject.loadOBJ(rkFileName))
        return false;

    // Load edges from object file.
    foreach(const Face* pkFace, m_kMapObject.faces())
    {
        const QVector< const QVector3D* >& kVertices = pkFace->vertices();
        for(quint8 i = 0; i < kVertices.count(); ++i)
        {
            const Edge3D kEdge(*kVertices.at(i), *kVertices.at((i + 1) % kVertices.count()));
            if(!kEdge.isValid())
                return false;

            // Insert edges to corresponding sets.
            if(m_kWalls.contains(kEdge))
            {
                m_kWalls.remove(kEdge);
                m_kGates.insert(kEdge);
            }
            else if(!m_kGates.contains(kEdge))
            {
                m_kWalls.insert(kEdge);
            }
        }
    }

    // Remove gates connected to walls.
    QSet< const QVector3D* > kWallVertices;

    foreach(const Edge3D& rkWall, m_kWalls)
    {
        kWallVertices.insert(&(rkWall.p1()));
        kWallVertices.insert(&(rkWall.p2()));
    }

    QSet< const Edge3D >::Iterator kIt = m_kGates.begin();
    while(kIt != m_kGates.end())
    {
        if(kWallVertices.contains(&(kIt->p1())) || kWallVertices.contains(&(kIt->p2())))
            kIt = m_kGates.erase(kIt);
        else
            ++kIt;
    }

    // Fill.
    foreach(const Edge3D& rkEdge, m_kGates)
    {
        m_kGraph.insert(&(rkEdge.p1()), &rkEdge);
        m_kGraph.insert(&(rkEdge.p2()), &rkEdge);
    }

    foreach(const Edge3D& rkEdge, m_kWalls)
    {
        m_kGraph.insert(&(rkEdge.p1()), &rkEdge);
        m_kGraph.insert(&(rkEdge.p2()), &rkEdge);
    }

    // Setup quadtrees.
    foreach(const Face* pkFace, m_kMapObject.faces())
        m_kQuadtreeWalkables.add(*pkFace);

    m_kQuadtreeWalkables.setBounds(m_kMapObject.minX(), m_kMapObject.minZ(),
                               m_kMapObject.maxX() - m_kMapObject.minX(),
                               m_kMapObject.maxZ() - m_kMapObject.minZ());

    foreach(const Edge3D& rkEdge, m_kGates)
        m_kQuadtreeGates.add(rkEdge);

    m_kQuadtreeGates.setBounds(m_kMapObject.minX(), m_kMapObject.minZ(),
                               m_kMapObject.maxX() - m_kMapObject.minX(),
                               m_kMapObject.maxZ() - m_kMapObject.minZ());

    foreach(const Edge3D& rkEdge, m_kWalls)
        m_kQuadtreeWalls.add(rkEdge);

    m_kQuadtreeWalls.setBounds(m_kMapObject.minX(), m_kMapObject.minZ(),
                               m_kMapObject.maxX() - m_kMapObject.minX(),
                               m_kMapObject.maxZ() - m_kMapObject.minZ());

    // Set state and return.
    m_bIsValid = true;
    return true;
}

void Map::paint() const
{
    // Check for validity.
    if(!m_kMapObject.isValid())
        return;

    // Pushing.
    glPushAttrib(GL_CURRENT_BIT);    

    // Paint graph.
    const qreal graphYOffset = 0.1;

    // Gates.
    glBegin(GL_LINES);
    glColor3f(0.0f, 0.2f, 0.0f);
    foreach(const Edge3D& rkGate, m_kGates)
    {
        glVertex3f(rkGate.p1().x(), rkGate.p1().y() + graphYOffset, rkGate.p1().z());
        glVertex3f(rkGate.p2().x(), rkGate.p2().y() + graphYOffset, rkGate.p2().z());
    }
    glEnd();

    // Walls.
    glBegin(GL_LINES);
    glColor3f(1.f, 0.2f, 0.f);
    foreach(const Edge3D& rkWall, m_kWalls)
    {
        glVertex3f(rkWall.p1().x(), rkWall.p1().y() + graphYOffset, rkWall.p1().z());
        glVertex3f(rkWall.p2().x(), rkWall.p2().y() + graphYOffset, rkWall.p2().z());
    }
    glEnd();

    // Popping.
    glPopAttrib();
}

QVector3D Map::snap(const QVector3D& rkPosition) const
{
    /*// Find the deepest related quadrant.
    const Quadtree< Navel >* pkQuadrant = m_kQuadtreeNavels.findAt(rkPosition);

    // The requested position is outside quadtree's boundaries.
    // It implies and exhaustive search.
    if(!pkQuadrant)
    {

    }

    // Find an useful quadrant, i.e containing at least one walkable navel.
    while(pkQuadrant->count() == 0 || bWalkable found

    // Keep position inside the quadtree's boundaries.
    QRectF kBoundaries =  m_kQuadtreeNavels.bounds();
    QVector3D kSnapped(qMax(kBoundaries.x(), qMin(kBoundaries.x() + kBoundaries.width(), rkPosition.x())),
                       rkPosition.y(),
                       qMax(kBoundaries.y(), qMin(kBoundaries.y() + kBoundaries.height(), rkPosition.z())));


    const Navel* pkNavel = findAt(kSnapped);

    Q_ASSERT



    // No need to snap because the position is already on a walkable navel.
    if(pkNavel && pkNavel->isWalkable())
        return rkPosition;

    // Otherwise find the closest snapping point.
    QMultiHash< const Navel*, const Navel* >::ConstIterator kIt = m_kDocksNavels.find(pkNavel)

    const Navel* pkBlockingNavel = findAt(rkPosition);
    m_kNavelToDockNavels.find(pkBlockingNavel);*/

    return rkPosition;
}

Path Map::findPath(const QVector3D& rkStart, const QVector3D& rkEnd) const
{
    const Face* pkStartWalkable = findAt(snap(rkStart));
    const Face* pkEndWalkable = findAt(snap(rkEnd));

    // DEBUG : This is for demo purpose only.
    // DELETE THIS HACK !.
    if(!pkStartWalkable || !pkEndWalkable)
        return Path();

    // Check if start and end positions are over the same face.
    if(pkStartWalkable == pkEndWalkable)
        return Path(rkStart, rkEnd);

    // Otherwise wrap the A* call with a connection / disconnection of start and end vertices.
    connect(rkStart, *pkStartWalkable, rkEnd, *pkEndWalkable);

    QLinkedList< const QVector3D* > kResult = AStar::instance()->run(m_kGraph, rkStart, rkEnd);

    disconnect();

    // Prepare path.
    Path kPath;
    foreach(const QVector3D* pkVertex, kResult)
        kPath.append(*pkVertex);

    // Finally try to smooth the path.
    return smooth(kPath);
}

bool Map::hover(const QVector3D& rkVertex, const Face& rkFace)
{
    const QVector< const QVector3D* > kVertices = rkFace.vertices();
    Q_ASSERT(kVertices.count() >= 3);

    qreal sign = 0;
    for(quint8 i = 0; i < kVertices.count(); ++i)
    {
        QVector3D kEdgeVector3D = *(kVertices.at((i + 1) % kVertices.count())) - *(kVertices.at(i));
        QVector3D kInnerVector3D = rkVertex - *(kVertices.at(i));
        qreal result = kEdgeVector3D.x() * kInnerVector3D.z() - kEdgeVector3D.z() * kInnerVector3D.x();

        // Find sign.
        if(sign == 0)
        {
            sign = result;
        }
        else if(result != 0 && (sign / qAbs(sign)) != (result / qAbs(result)))
        {
            return false;
        }
    }

    return true;
}

const Face* Map::findAt(const QVector3D& rkPosition) const
{
    const Quadtree< Face >* pkQuadtree = m_kQuadtreeWalkables.findAt(rkPosition);

    // The quadtree doesn't contain the position.
    if(!pkQuadtree)
        return NULL;

    // Check potential faces.
    foreach(const Face* pkFace, pkQuadtree->items())
    {
        if(hover(rkPosition, *pkFace))
            return pkFace;
    }

    return NULL;
}

void Map::connect(const QVector3D& rkStart, const Face& rkStartFace,
                  const QVector3D& rkEnd, const Face& rkEndFace) const
{
    // Check that start and end are in walkables faces.
    Q_ASSERT(hover(rkStart, rkStartFace));
    Q_ASSERT(hover(rkEnd, rkEndFace));

    const QVector< const QVector3D* >& kStartFaceVertices = rkStartFace.vertices();
    const QVector< const QVector3D* >& kEndFaceVertices = rkEndFace.vertices();

    // Create extra edges.
    foreach(const QVector3D* pkStartFaceVertex, kStartFaceVertices)
        m_kExtras.insert(Edge3D(rkStart, *pkStartFaceVertex));

    foreach(const QVector3D* pkEndFaceVertex, kEndFaceVertices)
        m_kExtras.insert(Edge3D(rkEnd, *pkEndFaceVertex));

    // Add newly create edges to the graph.
    foreach(const Edge3D& rkEdge, m_kExtras)
    {
        m_kGraph.insert(&(rkEdge.p1()), &rkEdge);
        m_kGraph.insert(&(rkEdge.p2()), &rkEdge);
    }
}

void Map::disconnect() const
{
    // Remove every extra connection from graph.
    foreach(const Edge3D& rkEdge, m_kExtras)
    {
        m_kGraph.remove(&(rkEdge.p1()), &rkEdge);
        m_kGraph.remove(&(rkEdge.p2()), &rkEdge);
    }

    // Remove extra edges.
    m_kExtras.clear();
}

Path& Map::smooth(Path& rkPath) const
{
    // No need to smooth a straight path.
    if(rkPath.count() < 3)
        return rkPath;

    // Smoothing loop.
    Path::Iterator kIt = rkPath.begin();
    while(kIt + 2 != rkPath.end())
    {
        const QVector3D& rkSource = *kIt;
        const QVector3D& rkTarget = *(kIt + 2);
        const Edge3D kCurrentEdge(rkSource, rkTarget);

        const QPointF kProjectedSource(rkSource.x(), rkSource.z());
        const QPointF kProjectedTarget(rkTarget.x(), rkTarget.z());
        const QLineF kProjectedCurrentEdge(kProjectedSource, kProjectedTarget);

        // Find the path edge quadrant.
        const Quadtree< Edge3D >* pkQuadrant = m_kQuadtreeWalls.findItem(kCurrentEdge);
        Q_ASSERT(pkQuadrant);

        // Check for intersection beetween walls and path edge.
        bool bErasable = true;

        const QSet< const Edge3D* >& rkWalls = pkQuadrant->items();
        foreach(const Edge3D* pkWall, rkWalls)
        {
            const QLineF kWallProjection(pkWall->p1().x(),
                                         pkWall->p1().z(),
                                         pkWall->p2().x(),
                                         pkWall->p2().z());

            // Check if there is any intersection with walls.
            if(kProjectedCurrentEdge.intersect(kWallProjection, NULL) == QLineF::BoundedIntersection)
            {
                ++kIt;
                bErasable = false;
                break;
            }
        }

        // No crossing found... the path edge can be removed.
        if(bErasable)
        {
            rkPath.erase(kIt + 1);
        }
    }

    return rkPath;
}
